#include "motor.h"
void motor_init(void)
{
  pwm_init(PWM_L, 17000, 0);
	pwm_init(PWM_R, 17000, 0);
	pwm_init(PWM_SERVO ,50, 0);//
  gpio_mode(P6_0,GPO_PP);
	gpio_mode(P6_4,GPO_PP);

}

void left_speed(int16 l_speed)
{
	if(l_speed>SPEED_MAX){l_speed=SPEED_MAX;}
	else if(l_speed<-SPEED_MAX){l_speed=-SPEED_MAX;}
  if(l_speed>0){DIR_L=0;pwm_duty(PWM_L, l_speed);}
	else{DIR_L=1;pwm_duty(PWM_L, -l_speed);}
  
}
void right_speed(int16 r_speed)
{
	if(r_speed>SPEED_MAX){r_speed=SPEED_MAX;}
	else if(r_speed<-SPEED_MAX){r_speed=-SPEED_MAX;}
  if(r_speed>0){DIR_R=0;pwm_duty(PWM_R, r_speed);}
	else{DIR_R=1;pwm_duty(PWM_R, -r_speed);}
  
}

void motor_speed(int16 L_speed,int16 R_speed)
{
    left_speed(L_speed);
	 right_speed(R_speed);

}
void Steer(int Angle)
{   // uint16 duty;
//	 duty = 1.5*10000/20;
//	 pwm_init(STEER_PIN, 50, duty);
    if(Angle>=LEFT_MAX)
       Angle=LEFT_MAX;
   else if(Angle<=RIGHT_MAX)
        Angle=RIGHT_MAX;
    pwm_duty(STEER_PIN, STEER_MID+Angle);  
    }